#include <termios.h>
#include <unistd.h>
#include <sys/timeb.h>
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/ioctl.h>

void logger(const char* format, ...)
{
    va_list ap;                    
    char msg[2048];                

    time_t t; struct tm tm; struct timeb tp; 
    ftime(&tp); t = tp.time; tm = *localtime(&t); 

    sprintf(msg, "[%02d-%02d-%d %02d:%02d:%02d %03d]: ", tm.tm_mon + 1, 
            tm.tm_mday, tm.tm_year + 1900, tm.tm_hour, tm.tm_min, 
            tm.tm_sec, tp.millitm); 
    va_start(ap, format);          
    vsnprintf(msg + strlen(msg), sizeof(msg)-strlen(msg), format, ap);
    va_end(ap);

    printf(msg);
    fflush(stdout);               
}

int serial_port_setup (int fd, speed_t baud_rate)
{
    struct termios stbuf;

    memset (&stbuf, 0, sizeof (stbuf)); 
    if (tcgetattr (fd, &stbuf) != 0) {
        perror("tcgetattr ");
        logger("FAILED TO GET SERIAL ATTR [%d]\n", fd);
        return -1;
    }

    stbuf.c_cflag &= ~(CBAUD | CSIZE | CSTOPB | CLOCAL | PARENB);
    stbuf.c_iflag &= ~(HUPCL | IUTF8 | IUCLC | ISTRIP | IXON | IXOFF | IXANY | ICRNL);
    stbuf.c_oflag &= ~(OPOST | OCRNL | ONLCR | OLCUC | ONLRET);
    stbuf.c_lflag &= ~(ICANON | ISIG | IEXTEN | ECHO | ECHOE | ECHOK | ECHONL);
    stbuf.c_lflag &= ~(NOFLSH | XCASE | TOSTOP | ECHOPRT | ECHOCTL | ECHOKE);
    stbuf.c_cc[VMIN] = 1;
    stbuf.c_cc[VTIME] = 0;
    stbuf.c_cc[VEOF] = 1;
    stbuf.c_cflag |= (baud_rate | CS8 | CREAD | 0 | 0);  /* No parity, 1 stop bit */
    errno = 0;
    if (tcsetattr (fd, TCSANOW, &stbuf) < 0) {
        perror("tcgetattr");
        logger("FAILED TO SET SERIAL ATTR [%d]\n", fd);
        return -1;
    }
    return 0;
}


int open_port(const char* port, speed_t baud_rate)
{
    int fd;
    struct termios termio;

    memset (&termio, 0, sizeof (termio));

    while(true) {
reopen:
        fd = open(port, O_RDWR | O_NDELAY | O_EXCL | O_NONBLOCK | O_NOCTTY);
        if(fd != -1)
            break;
        sleep(1);
        logger("Unable to open the port %s\n", port);
    }

    if (ioctl (fd, TIOCEXCL) < 0) {
        logger ("Could not lock serial device[%s]: err[%s]",
                port, strerror(errno));
        close(fd);
        goto reopen;
    }

    /* Flush any waiting IO */
    tcflush (fd, TCIOFLUSH);

    if(serial_port_setup(fd, baud_rate) < 0) {
        close(fd);
        goto reopen;
    }

    fcntl(fd, F_SETFL, 0);
    logger("Successfuly connected PORT[%s]\n", port);
    return fd;
}

int main(int argc, char **argv)
{	
    uint8_t value;
    int read_bytes = 0;
    int must_read = 0;

    bool first_iteration = true;

    if(argc != 2) {
        printf("Usage: %s <port>\n", argv[0]);
        return 0;
    }

    int fd = open_port(argv[1], B38400);

start:
    logger("\n");
    logger("START#");
    while(true) {
        if(read(fd, &value, 1) <= 0) {
            close(fd);
            logger("\n");
            logger("PORT[%s] was disconnected, reconnecting...\n", argv[1]);
            fd = open_port(argv[1], B38400);
            goto start;
            continue;
        }

        if(first_iteration && value != 58) { // skip garbage
            printf("!%ld! ", value);
            continue;
        }

        if(value == 58 && !read_bytes) {
            must_read = 3;
            printf("\n"); logger("");
            first_iteration = false;
        }

        must_read--;
        read_bytes++;

        if(read_bytes == 3 && !must_read)
            must_read = value;

        if(!must_read) {
            read_bytes = 0;
            first_iteration = true;
        }
        printf("%ld ", value);
    }

    return 0;
}
